#include "./handler_task.h"
#include "../pub_service/ota_service.h"
#include "../heat_driver/motors.h"
#include "../pub_driver/cmd_can.h"
#include "../heat_driver/ctrl.h"
#include "../cfg/heat_info.h"
#include "../heat_driver/sensor.h"
#include "./draw_hot_task.h"
#include "./draw_tank_task.h"
#include "./heat_task.h"
#include "../utils/alg.h"


namespace heat_task {

using namespace pub_service;
using namespace conn;
using namespace pub_driver;
using namespace heat_driver;
using namespace cfg;

HandlerTask handlerTask;

void HandlerTask::init_start() {
    m_mq.init();
    start();
}

static void on_film_ctrl(const Frame &frame) {
    uint8_t v;
    Error err = frame.parse(v);
    if (err) {
        goto finish;
    }
    switch (v) {
        case 0: err = motors.film_up(); break;
        case 1: err = motors.film_down(); break;
        default: err = ec::pub::INVALID; break;
    }
finish:
    cmdCAN.fast_res(frame, err);
}

static void on_parts_ctrl(const Frame &frame) {
    uint8_t n, v;
    Error err = frame.parse(n, v);
    if (err) {
        goto finish;
    }
    if ((n > 15) || (v > 1)) {
        err = ec::pub::INVALID;
        goto finish;
    }
    if (v == 0) {
        ctrl.open( static_cast<Ctrl::Parts>(n) );
    } else {
        ctrl.close( static_cast<Ctrl::Parts>(n) );
    }
finish:
    cmdCAN.fast_res(frame, err);
}

static void on_stir_cap_ctrl(const Frame &frame) {
    uint8_t v;
    Error err = frame.parse(v);
    if (err) {
        goto finish;
    }
    switch (v) {
        case 0: err = motors.stir_cap_open(); break;
        case 1: err = motors.stir_cap_close(); break;
        default: err = ec::pub::INVALID; break;
    }
finish:
    cmdCAN.fast_res(frame, err);
}

static void on_hold_ctrl(const Frame &frame) {
    uint8_t v;
    Error err = frame.parse(v);
    if (err) {
        goto finish;
    }
    switch (v) {
        case 0: err = motors.hold_left(); break;
        case 1: err = motors.hold_right(); break;
        default: err = ec::pub::INVALID; break;
    }
finish:
    cmdCAN.fast_res(frame, err);
}

static void on_stir_motor_ctrl(const Frame &frame) {
    uint8_t v;
    Error err = frame.parse(v);
    if (err) {
        goto finish;
    }
    switch (v) {
        case 0: motors.stir_open_ccw(); break;
        case 1: motors.stir_open_cw(); break;
        case 2: motors.stir_close(); break;
        default: err = ec::pub::INVALID; break;
    }
finish:
    cmdCAN.fast_res(frame, err);
}

static void on_set_draw(const Frame &frame) {
    uint16_t tank, hot;
    Error err = frame.parse(tank, hot);
    if (err) {
        goto finish;
    }
    heatInfo->tank_draw_time = tank * 60 * 1000;
    heatInfo->hot_draw_time = hot * 60 * 1000;
    err = heatInfo.save();
finish:
    cmdCAN.fast_res(frame, tank, hot);
}

static void on_set_heat(const Frame &frame) {
    uint16_t water_time, steam_time;
    uint8_t water_temp, steam_temp;
    Error err = frame.parse(
        water_time, steam_time, water_temp, steam_temp
    );
    if (err) {
        goto finish;
    }
    heatInfo->hot_water_heat_time = water_time * 60 * 1000;
    heatInfo->hot_steam_heat_time = steam_time * 60 * 1000;
    heatInfo->hot_water_temp = water_temp;
    heatInfo->hot_steam_temp = steam_temp;
    err = heatInfo.save();
finish:
    cmdCAN.fast_res(frame, err);
}

static void on_set_flow(const Frame &frame) {
    uint16_t flow;
    Error err = frame.parse(flow);
    if (err) {
        goto finish;
    }
    if (flow == 0) {
        err = ec::pub::INVALID;
        goto finish;
    }
    heatInfo->flow = flow;
    err = heatInfo.save();
finish:
    cmdCAN.fast_res(frame, err);
}

static Error draw_water(uint16_t ml) {
    Error err;
    sensor.flow_reset();
    uint16_t last = 0;
    uint16_t cur = 0;
    uint32_t time_count = 0;
    uint32_t time_count_2 = 0;
    ctrl.open(Ctrl::Parts::WATER_PUMP);
    for (;;) {
        time_count_2 += 50;
        time_count += 50;
        osDelay(50);

        cur = sensor.flow();

        if (cur >= ml) {
            break;
        }

        if (time_count_2 >= 3000) {
            if ((cur - last) < 5) {
                err = ec::heat::FLOW_FAIL;
                break;
            }
            time_count_2 = 0;
            last = cur;
        }
        
        if (time_count >= 120 * 1000) {
            err = ec::heat::FLOW_TIMEOUT;
            break;
        }
    }
    ctrl.close(Ctrl::Parts::WATER_PUMP);
    return err;
}

static void on_draw_ctrl(const Frame &frame) {
    uint8_t t;
    uint16_t v;
    Error err = frame.parse(t, v);
    if (err) {
        goto finish;
    }
    switch (t) {
        case 0:
        case 1: ctrl.open(Ctrl::Parts::V5); ctrl.open(Ctrl::Parts::V4); break;
        case 2: ctrl.open(Ctrl::Parts::V3); break;
        default: { 
            err = ec::pub::INVALID; 
            goto finish;
        }
    }
    err = draw_water(v);
finish:
    ctrl.close(Ctrl::Parts::V5);
    ctrl.close(Ctrl::Parts::V4);
    ctrl.close(Ctrl::Parts::V3);
    ctrl.close(Ctrl::Parts::WATER_PUMP);
    cmdCAN.fast_res(frame, err);
}

static void on_heat_draw_ctrl(const Frame &frame) {
    uint8_t v;
    Error err = frame.parse(v);
    if (err) {
        goto finish;
    }
    switch (v) {
        case 0: drawTankTask.idle(); break;
        case 1: drawTankTask.stop(); break;
        case 2: drawHotTask.idle(); break;
        case 3: drawHotTask.stop(); break;
        case 4: heatTask.idle(); break;
        case 5: heatTask.stop(); break;
        default: err = ec::pub::INVALID; break;
    }

finish:
    cmdCAN.fast_res(frame, err);
}

void HandlerTask::run() {

    Frame frame;
    for (;;) {
        m_mq.poll(frame);
        task_info.seq = frame.seq_cmd();
        task_info.cmd = frame.cmd();

        switch (frame.cmd()) {
            case proto::heat::HEAT_DRAW_CTRL: on_heat_draw_ctrl(frame); break;
            case proto::heat::HOT_DRAW: on_draw_ctrl(frame); break;
            case proto::heat::SET_FLOW: on_set_flow(frame); break;
            case proto::heat::SET_DRAW: on_set_draw(frame); break;
            case proto::heat::SET_HEAT: on_set_heat(frame); break;
            case proto::heat::STIR_MOTOR_CTRL: on_stir_motor_ctrl(frame); break;
            case proto::heat::HOLD_CTRL: on_hold_ctrl(frame); break;
            case proto::heat::STIR_CAP_CTRL: on_stir_cap_ctrl(frame); break;
            case proto::heat::PARTS_CTRL: on_parts_ctrl(frame); break;
            case proto::heat::FILM_CTRL: on_film_ctrl(frame); break;
            case proto::pub::OTA_START: pub_service::ota_start(frame); break;
            case proto::pub::OTA_TRANSLATE: pub_service::ota_translate(frame); break;
            case proto::pub::OTA_COMPLETE: pub_service::ota_complete(frame); break;
        }
    
        task_info.reset();
    }
}

}
